/*
 * @brief Sensor Hub demo sensors (implementation file)
 *
 * @note
 * Copyright(C) NXP Semiconductors, 2015
 * All rights reserved.
 *
 * @par
 * Software that is described herein is for illustrative purposes only
 * which provides customers with programming information regarding the
 * LPC products.  This software is supplied "AS IS" without any warranties of
 * any kind, and NXP Semiconductors and its licensor disclaim any and
 * all warranties, express or implied, including all implied warranties of
 * merchantability, fitness for a particular purpose and non-infringement of
 * intellectual property rights.  NXP Semiconductors assumes no responsibility
 * or liability for the use of the software, conveys no license or rights under any
 * patent, copyright, mask work right, or any other intellectual property rights in
 * or to any products. NXP Semiconductors reserves the right to make changes
 * in the software without notification. NXP Semiconductors also makes no
 * representation or warranty that such application will be suitable for the
 * specified use without further testing or modification.
 *
 * @par
 * Permission to use, copy, modify, and distribute this software and its
 * documentation is hereby granted, under NXP Semiconductors' and its
 * licensor's relevant copyrights in the software, without fee, provided that it
 * is used in conjunction with NXP Semiconductors microcontrollers.  This
 * copyright, permission, and disclaimer notice must appear in all copies of
 * this code.
 */

#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <stdbool.h>
#include <assert.h>
#include <math.h>

#include <GL/gl.h>
#include <GL/glu.h>
#include <GL/glut.h>

#include "hostif_protocol.h"

#include "trace.h"
#include "gl_text.h"
#include "gl_menu.h"
#include "misc.h"

#include "sensors.h"

/*****************************************************************************
 * Public types/enumerations/variables
 ****************************************************************************/

/*****************************************************************************
 * Private types/enumerations/variables
 ****************************************************************************/
#define	TS_BACK		25000					/* set timestamp backstep report at 25ms */
#define	TS_JUMP		1500000					/* set timestamp jump report at 1500ms */

/*
 *******************************************************************************
 *	These are the sensors (by sensor ID) that are
 *	supported by the BSX-lite library:
 *	- LPCSH_SENSOR_ID_ACCELEROMETER
 *	- LPCSH_SENSOR_ID_ACCELEROMETER_UNCALIBRATED
 *	- LPCSH_SENSOR_ID_MAGNETIC_FIELD
 *	- LPCSH_SENSOR_ID_MAGNETIC_FIELD_UNCALIBRATED
 *	- LPCSH_SENSOR_ID_GYROSCOPE
 *	- LPCSH_SENSOR_ID_GYROSCOPE_UNCALIBRATED
 *	- LPCSH_SENSOR_ID_LIGHT
 *	- LPCSH_SENSOR_ID_PRESSURE (includes temperature)
 *	- LPCSH_SENSOR_ID_PROXIMITY
 *	- LPCSH_SENSOR_ID_ORIENTATION
 *	- LPCSH_SENSOR_ID_ROTATION_VECTOR
 *
 * Note:
 *	The rotation vector sensor is a special case.
 *	It drives the teapot display and can have two different HUD displays
 *	the rotation vector and the rotation matrix.
 *******************************************************************************
*/
static	struct lpcsh_motion_sensor_node			m_accel_cal;
static	struct lpcsh_motion_uncal_sensor_node	m_accel_uncal;
static	struct lpcsh_motion_sensor_node			m_mag_cal;
static	struct lpcsh_motion_uncal_sensor_node	m_mag_uncal;
static	struct lpcsh_motion_sensor_node			m_gyro_cal;
static	struct lpcsh_motion_uncal_sensor_node	m_gyro_uncal;
static	struct lpcsh_motion_sensor_node			m_light;
static	struct lpcsh_motion_sensor_node			m_pressure;
static	struct lpcsh_motion_sensor_node			m_proximity;
static	struct lpcsh_quaternion_node			m_rotation;

static	uint32_t	timestamp;
static	char		text_buffer[256];

/*****************************************************************************
 * Private functions
 ****************************************************************************/
static uint32_t ts_save;
static bool		ts_init = false;

static void ts_assert(uint32_t ts)
{
	if (ts_init == false) {														// Initialization variable
		ts_init = true;															// Set to true (do this once)
		ts_save = ts;															// Initialize the timestamp
	}

	if (ts == 0)																// Check if timestamp is zero
		trace(TRACE_INFO, "timestamp zero");									// if so, trace it
	if (ts < ts_save-TS_BACK)													// Check if timestamp has jumped backward more than 1ms
		trace(TRACE_INFO, "timestamp backstep: %d", abs(ts_save-ts));			// if so, trace it
	if (ts > ts_save+TS_JUMP)													// Check if timestamp has jumped forward more than 25ms
		trace(TRACE_INFO, "timestamp jump: %d", abs(ts-ts_save));				// if so, trace it

	ts_save = ts;
}

/*****************************************************************************
 * Public functions
 ****************************************************************************/

/*
 *******************************************************************************
 * Accelerometer sensor (calibrated) processing
 *******************************************************************************
 */
void disp_timestamp(TEXT_INFO_T* t_info)
{
	if (gl_menu_ts_get() == true)
		sprintf(text_buffer, "BSXLite timestamp: %s", time_fmt(timestamp));
	else
		sprintf(text_buffer, "BSXLite timestamp (raw): %08u", timestamp);

	gl_text_print(t_info, text_buffer);
}

/*
 *******************************************************************************
 * Accelerometer sensor (calibrated) processing
 * All the rest of the sensors follow a similar pattern.
 * This is the only block that is commented.
 *******************************************************************************
 */
void accel_cal_init(void)
{
	memset(&m_accel_cal, 0, sizeof(m_accel_cal));								/* clear the local data structure */
}
void accel_cal_data_acq(uint8_t* rx)
{
	uint8_t id;																	/* packet ID */
	struct lpcsh_sensor_node* pData;											/* packet pointer */

	pData = (struct lpcsh_sensor_node*)rx;										/* point / cast to packet */
	id = pData->header.sensorId;												/* get the ID */
	assert(id == LPCSH_SENSOR_ID_ACCELEROMETER);								/* assert that the ID is correct */
	timestamp = pData->header.timeStamp;										/* get the timestamp */
	ts_assert(timestamp);														/* verify the timestamp */
	memcpy(&m_accel_cal, &(pData->data.sensorData), sizeof(m_accel_cal));		/* copy the data to a local structure */
}
void accel_cal_update_hud(TEXT_INFO_T* t_info)
{
	float qx = (float)m_accel_cal.Data[0]/9.0f;									/* Calculate X acceleration in g */
	float qy = (float)m_accel_cal.Data[1]/9.0f;									/* Calculate Y acceleration in g */
	float qz = (float)m_accel_cal.Data[2]/9.0f;									/* Calculate Z acceleration in g */

	gl_text_print(t_info, "Accelerometer:");									/* Display heading */
	sprintf(text_buffer, "X:%-8.3f Y:%-8.3f Z:%-8.3f", qx, qy, qz);				/* Format data */
	gl_text_print(t_info, text_buffer);											/* Display data */
	gl_text_blank(t_info);														/* Place a blank line after the data */
}

/*
 *******************************************************************************
 * Accelerometer sensor (uncalibrated) processing
 *******************************************************************************
 */
void accel_uncal_init(void)
{
	memset(&m_accel_uncal, 0, sizeof(m_accel_uncal));
}
void accel_uncal_data_acq(uint8_t* rx)
{
	uint8_t id;
	struct lpcsh_sensor_node* pData;

	pData = (struct lpcsh_sensor_node*)rx;
	id = pData->header.sensorId;
	assert(id == LPCSH_SENSOR_ID_ACCELEROMETER_UNCALIBRATED);
	timestamp = pData->header.timeStamp;
	ts_assert(timestamp);
	memcpy(&m_accel_uncal, &(pData->data.uncal_sensorData), sizeof(m_accel_uncal));
}
void accel_uncal_update_hud(TEXT_INFO_T* t_info)
{
	float qx = (float)m_accel_cal.Data[0];										/* Calculate X acceleration in g */
	float qy = (float)m_accel_cal.Data[1];										/* Calculate Y acceleration in g */
	float qz = (float)m_accel_cal.Data[2];										/* Calculate Z acceleration in g */

	gl_text_print(t_info, "Accelerometer (uncal):");
	sprintf(text_buffer, "X:%-8.3f Y:%-8.3f Z:%-8.3f", qx, qy, qz);				/* Format data */
	gl_text_print(t_info, text_buffer);											/* Display data */
	gl_text_blank(t_info);														/* Place a blank line after the data */
}

/*
 *******************************************************************************
 * Magnetometer sensor (calibrated) processing
 *******************************************************************************
 */
void mag_cal_init(void)
{
	memset(&m_mag_cal, 0, sizeof(m_mag_cal));
}
void mag_cal_data_acq(uint8_t* rx)
{
	uint8_t id;
	struct lpcsh_sensor_node* pData;

	pData = (struct lpcsh_sensor_node*)rx;
	id = pData->header.sensorId;
	assert(id == LPCSH_SENSOR_ID_MAGNETIC_FIELD);
	timestamp = pData->header.timeStamp;
	ts_assert(timestamp);
	memcpy(&m_mag_cal, &(pData->data.sensorData), sizeof(m_mag_cal));
}
void mag_cal_update_hud(TEXT_INFO_T* t_info)
{
	gl_text_print(t_info, "Magnetometer:");
	sprintf(text_buffer, "%8d %8d %8d", m_mag_cal.Data[0], m_mag_cal.Data[1], m_mag_cal.Data[2]);
	gl_text_print(t_info, text_buffer);
	gl_text_blank(t_info);
}

/*
 *******************************************************************************
 * Magnetometer sensor (uncalibrated) processing
 *******************************************************************************
 */
void mag_uncal_init(void)
{
	memset(&m_mag_uncal, 0, sizeof(m_mag_uncal));
}
void mag_uncal_data_acq(uint8_t* rx)
{
	uint8_t id;
	struct lpcsh_sensor_node* pData;

	pData = (struct lpcsh_sensor_node*)rx;
	id = pData->header.sensorId;
	assert(id == LPCSH_SENSOR_ID_MAGNETIC_FIELD_UNCALIBRATED);
	timestamp = pData->header.timeStamp;
	ts_assert(timestamp);
	memcpy(&m_mag_uncal, &(pData->data.uncal_sensorData), sizeof(m_mag_uncal));
}
void mag_uncal_update_hud(TEXT_INFO_T* t_info)
{
	gl_text_print(t_info, "Magnetometer (uncal):");
	sprintf(text_buffer, "%8d %8d %8d", m_mag_uncal.Data[0], m_mag_uncal.Data[1], m_mag_uncal.Data[2]);
	gl_text_print(t_info, text_buffer);
	gl_text_blank(t_info);
}

/*
 *******************************************************************************
 * Gyroscope (calibrated) processing
 *******************************************************************************
 */
void gyro_cal_init(void)
{
	memset(&m_gyro_cal, 0, sizeof(m_gyro_cal));
}
void gyro_cal_data_acq(uint8_t* rx)
{
	uint8_t id;
	struct lpcsh_sensor_node* pData;

	pData = (struct lpcsh_sensor_node*)rx;
	id = pData->header.sensorId;
	assert(id == LPCSH_SENSOR_ID_GYROSCOPE);
	timestamp = pData->header.timeStamp;
	ts_assert(timestamp);
	memcpy(&m_gyro_cal, &(pData->data.sensorData), sizeof(m_gyro_cal));
}
void gyro_cal_update_hud(TEXT_INFO_T* t_info)
{
	gl_text_print(t_info, "Gyroscope:");
	sprintf(text_buffer, "%8d %8d %8d", m_gyro_cal.Data[0], m_gyro_cal.Data[1], m_gyro_cal.Data[2]);
	gl_text_print(t_info, text_buffer);
	gl_text_blank(t_info);
}

/*
 *******************************************************************************
 * Gyroscope (uncalibrated) processing
 *******************************************************************************
 */
void gyro_uncal_init(void)
{
	memset(&m_gyro_uncal, 0, sizeof(m_gyro_uncal));
}
void gyro_uncal_data_acq(uint8_t* rx)
{
	uint8_t id;
	struct lpcsh_sensor_node* pData;

	pData = (struct lpcsh_sensor_node*)rx;
	id = pData->header.sensorId;
	assert(id == LPCSH_SENSOR_ID_GYROSCOPE_UNCALIBRATED);
	timestamp = pData->header.timeStamp;
	ts_assert(timestamp);
	memcpy(&m_gyro_uncal, &(pData->data.uncal_sensorData), sizeof(m_gyro_uncal));
}
void gyro_uncal_update_hud(TEXT_INFO_T* t_info)
{
	gl_text_print(t_info, "Gyroscope (uncal):");
	sprintf(text_buffer, "%8d %8d %8d", m_gyro_uncal.Data[0], m_gyro_uncal.Data[1], m_gyro_uncal.Data[2]);
	gl_text_print(t_info, text_buffer);
	gl_text_blank(t_info);
}

/*
 *******************************************************************************
 * Light sensor processing
 *******************************************************************************
 */
void light_init(void)
{
	memset(&m_light, 0, sizeof(m_light));
}
void light_data_acq(uint8_t* rx)
{
	uint8_t id;
	struct lpcsh_sensor_node* pData;

	pData = (struct lpcsh_sensor_node*)rx;
	id = pData->header.sensorId;
	assert(id == LPCSH_SENSOR_ID_LIGHT);
	timestamp = pData->header.timeStamp;
	ts_assert(timestamp);
	memcpy(&m_light, &(pData->data.sensorData), sizeof(m_light));
}
void light_update_hud(TEXT_INFO_T* t_info)
{
	gl_text_print(t_info, "Light Sensor:");
	sprintf(text_buffer, "%8d", m_light.Data[0]);
	gl_text_print(t_info, text_buffer);
	gl_text_blank(t_info);
}

/*
 *******************************************************************************
 * Atmospheric pressure sensor processing
 *******************************************************************************
 */
void pressure_init(void)
{
	memset(&m_pressure, 0, sizeof(m_pressure));
}
void pressure_data_acq(uint8_t* rx)
{
	uint8_t id;
	struct lpcsh_sensor_node* pData;

	pData = (struct lpcsh_sensor_node*)rx;
	id = pData->header.sensorId;
	assert(id == LPCSH_SENSOR_ID_PRESSURE);
	timestamp = pData->header.timeStamp;
	ts_assert(timestamp);
	memcpy(&m_pressure, &(pData->data.sensorData), sizeof(m_pressure));
}
void pressure_update_hud(TEXT_INFO_T* t_info)
{
	uint32_t	t_press;
	float		f_press;

	t_press = (uint32_t)((uint16_t)m_pressure.Data[1]<<16 | (uint16_t)m_pressure.Data[0]);
	f_press = (float)t_press / 100.0f;

	gl_text_print(t_info, "Pressure / Temperature:");
	sprintf(text_buffer, "Pressure: %-5.2f hPa", f_press);
	gl_text_print(t_info, text_buffer);
	sprintf(text_buffer, "Temperature: %-5.2f C", (float)m_pressure.Data[2]/100.0f);
	gl_text_print(t_info, text_buffer);
	gl_text_blank(t_info);
}

/*
 *******************************************************************************
 * Proximity sensor processing
 *******************************************************************************
 */
void proximity_init(void)
{
	memset(&m_proximity, 0, sizeof(m_proximity));
}
void proximity_data_acq(uint8_t* rx)
{
	uint8_t id;
	struct lpcsh_sensor_node* pData;

	pData = (struct lpcsh_sensor_node*)rx;
	id = pData->header.sensorId;
	assert(id == LPCSH_SENSOR_ID_PROXIMITY);
	timestamp = pData->header.timeStamp;
	ts_assert(timestamp);
	memcpy(&m_proximity, &(pData->data.sensorData), sizeof(m_proximity));
}
void proximity_update_hud(TEXT_INFO_T* t_info)
{
	gl_text_print(t_info, "Proximity:");
	sprintf(text_buffer, "%8d", m_proximity.Data[0]);
	gl_text_print(t_info, text_buffer);
	gl_text_blank(t_info);
}

/*
 *******************************************************************************
 * Rotation sensor processing
 *******************************************************************************
 */
void rotation_init(void)
{
	memset(&m_rotation, 0, sizeof(m_rotation));
}
void rotation_data_acq(uint8_t* rx)
{
	uint8_t id;
	struct lpcsh_sensor_node* pData;

	pData = (struct lpcsh_sensor_node*)rx;
	id = pData->header.sensorId;
	assert(id == LPCSH_SENSOR_ID_ROTATION_VECTOR);
	timestamp = pData->header.timeStamp;
	ts_assert(timestamp);
	memcpy(&m_rotation, &(pData->data.quaternionData), sizeof(m_rotation));
}
void rotation_update_hud(TEXT_INFO_T* t_info)
{
	float qz = (float)m_rotation.X / 16384.0f;
	float qy = (float)m_rotation.Y / 16384.0f;
	float qx = (float)m_rotation.Z / 16384.0f;

	gl_text_print(t_info, "Rotation vector:");
	sprintf(text_buffer, "X:%-8.3f Y:%-8.3f Z:%-8.3f", qx, qy, qz);
	gl_text_print(t_info, text_buffer);
	gl_text_blank(t_info);
}
